function [dxdt,s]=rhs(t,x,p)
    s=struct();
%     q0=[0.1*t+1;0.5*t+2];
%     dq0=[0.1;0.5];
%     d2q0=[0,0];
    q0=[0.1*sin(t)+1;0.5*sin(t)+2];
    dq0=[0.1*cos(t);0.5*cos(t)];
    d2q0=[-0.1*cos(t);-0.5*sin(t)];
    ai=p.Theta;
    qi=x(1:2);
    dqi=x(3:4);
    hatTheta=x(5:9);
    dxdt=zeros(size(x));
    Mi=zeros(2,2);
    Mi(1,1)=ai(1)+ai(2)+2*ai(3)*cos(qi(2));
    Mi(1,2)=ai(2)+ai(3)*cos(qi(2));
    Mi(2,1)=ai(2)+ai(3)*cos(qi(2));
    Mi(2,2)=ai(2);
    Ci=zeros(2,2);
    Ci(1,1)=-ai(3)*sin(qi(2))*dqi(2);
    Ci(1,2)=-ai(3)*sin(qi(2))*(dqi(1)+dqi(2));
    Ci(2,1)=ai(3)*sin(qi(2))*dqi(1);
    Gi=zeros(2,1);
    g=9.8;
    Gi(1,1)=ai(4)*g*cos(qi(1))+ai(5)*g*cos(qi(1)+qi(2));
    Gi(2,1)=ai(5)*g*cos(qi(1)+qi(2));

    zeta=dq0-p.alpha*(qi-q0);
    dzeta=d2q0-p.alpha*(dqi-dq0);
    x=dzeta;
    y=zeta;
    Yi=zeros(2,4);
    Yi(1,1)=x(1);
    Yi(1,2)=x(1)+x(2);
    Yi(1,3)=2*cos(qi(2))*x(1)+cos(qi(2))*x(2)-sin(qi(2))*dqi(2)*y(1)-sin(qi(2))*(dqi(1)+dqi(2))*y(2);
    Yi(1,4)=g*cos(qi(1));
    Yi(1,5)=g*cos(qi(1)+qi(2));
    Yi(2,2)=x(1)+x(2);
    Yi(2,3)=cos(qi(2))*x(1)+sin(qi(2))*dqi(1)*y(1);
    Yi(2,5)=g*cos(qi(1)+qi(2));

%     Yi=(Mi*dzeta+Ci*zeta+Gi)/p.Theta; %理论上Yi 应该等于这个

    si=dqi-zeta;
    ui=-p.K*si+Yi*hatTheta;
    dhatTheta=-inv(p.Lambda)*Yi'*si;
    dxdt(1:2)=dqi;
    dxdt(3:4)=Mi\(ui-Ci*dqi-Gi);
    dxdt(5:9)=dhatTheta;
    
    s.Y=Yi;
    s.s=si;
    s.q0=q0;
    s.debugP3=(Yi*p.Theta-(Mi*dzeta+Ci*zeta+Gi));   %检测参数线性化性质是否实现正确
end